function positionk = Locationupdate(v_k_1n,v_kn,position_k_1)
%已知量：上一时刻的位置参数：h、phi、lambda
%已知量：上一时刻和当前时刻的垂向、北向、东向速度：vD、vE、vE
%位置更新
%%
%变量初始化
    phi_k_1    = position_k_1(1);
    lambda_k_1 = position_k_1(2);
    h_k_1      = position_k_1(3);
    vN_k_1n    = v_k_1n(1);
    vE_k_1n    = v_k_1n(2);
    vD_k_1n    = v_k_1n(3);
    vN_kn      = v_kn(1);
    vE_kn      = v_kn(2);
    vD_kn      = v_kn(3);
%%
%高程更新
    detat=0.005;
    h_k=h_k_1-0.5*(vD_k_1n+vD_kn)*detat;

%纬度更新
    h_bar   = 0.5*(h_k+h_k_1);
    R_k_1   = rmnGet(phi_k_1);
    RM_k_1  = R_k_1(1);
    phi_k   = phi_k_1 + 0.5*detat*(vN_kn+vN_k_1n)/(RM_k_1+h_bar);

%经度更新
    h_bar      = 0.5*(h_k+h_k_1);
    phi_bar    = 0.5*(phi_k + phi_k_1);
    R_k_1_2    = rmnGet(phi_bar);
    R_N_k_1_2  = R_k_1_2(2);%RN,k−1,2 表示用中间时刻的纬度计算子午圈半径RM
    lambda_k   = lambda_k_1 + 0.5*detat*(vE_kn+vE_k_1n)/((R_N_k_1_2+h_bar)*cos(phi_bar));

    positionk  = [phi_k; lambda_k; h_k];
end

function R = rmnGet(phi)
%已知量：纬度
%获得某个纬度处的子午圈半径：RM、卯酉圈半径：RN;
    a    = 6378137.000000000000;
    e    = 0.08181919104;
    RM=(a * (1 - e^2)) / (1 - e^2 * sin(phi)^2)^(3/2);
    RN=a / sqrt(1 - e^2 * sin(phi)^2);
    R=[RM; RN];
end